//------------------------------------------------------------------------------
// TEAM 1280 - SAN RAMON VALLEY HIGH SCHOOL RAGIN' C-BISCUITS
// 2012 REBOUND RUMBLE ROBOT
//------------------------------------------------------------------------------
#include "WPILib.h"   // Instruction to preprocessor to include the WPI Library
// header file
#include <cmath>

#define CONSOLE
#include "H/CameraLights.h"
#include "H/DiskLoader.h"
#include "H/DiskStorage.h"
#include "H/DiskShooter.h"
#include "H/RobotAimer.h"
#include "H/TankDrive.h"
#include "H/Transmission.h"
#include "H/Elevator.h"
#include "H/Tipper.h"

//------------------------------------------------------------------------------
// DEFINE BuiltinDefaultCode CLASS
//------------------------------------------------------------------------------
// Derived from IterativeRobot class in WPILib.h.  Inherits attributes and
// functions of the base IterativeRobot class.
// Extends class through:
// - Overrides to virtual methods contained in the IterativeRobot base class
// - Methods specific to the 2012 Team 1280 Rebound Rumble robot
// - Constants used by class
// - Pointers to required objects
// - Additional local variables required
//------------------------------------------------------------------------------
class BuiltinDefaultCode : public IterativeRobot
{
	//--------------------------------------------------------------------------
	// DECLARATION OF PUBLIC METHODS
	//--------------------------------------------------------------------------
public:
	// Constructor and deconstructor methods
	BuiltinDefaultCode();
	~BuiltinDefaultCode();
	//----------------------------------------------------------------------
	// OVERRIDES TO IterativeRobot BASE CLASS VIRTUAL METHODS
	//----------------------------------------------------------------------
	// Robot and state initialization methods
	void   RobotInit();
	void   DisabledInit();
	void   AutonomousInit();
	void   TeleopInit();
	// Robot periodic methods performed in loops
	void   DisabledPeriodic();
	void   AutonomousPeriodic();
	void   TeleopPeriodic();

	//----------------------------------------------------------------------
	// CUSTOM METHODS SPECIFIC TO TEAM 1280 ROBOT
	//----------------------------------------------------------------------
	// Initialization and reset methods
	// Driver station and robot input gathering methods
	void   GetDriverStationInput();
	void   GetRobotSensorInput();
	void   GetAutoModeSwitches();
	void   ShowDSValues();
	void   ShowRobotValues();
	// Miscellaneous robot function methods
	INT32  GetTime();
	// Autonomous mode methods
	void   RunAutonomousMode();
	void   AMDriveRobot();
	void   AMPickUpDisk();
	void   AMShootOriginalDisks();
	void   AMShootExtraDisks();

	//Public Objects
	TankDrive		*pDriveMotors;

private:
	//----------------------------------------------------------------------
	// CONSTANTS USED IN CLASS
	//----------------------------------------------------------------------
	// DRIVER STATION PORTS AND CHANNELS
	//----------------------------------------------------------------------
	// Driver Station Joystick ports
	static const UINT32 LEFT_JS_PORT          =  1;
	static const UINT32 RIGHT_JS_PORT         =  2;
	static const UINT32 CCI_PORT              =  3;  // eStop Robots CCI Inputs

	// Driver Station CCI Channels (Uses joystick button references)
	static const UINT32 LED_LIGHTS_ON         =  1;  // Game piece light switches
	static const UINT32 AUTO_TARGET_CH		  =  2;
	static const UINT32 SHOOT_DISK_CH         =  3;
	static const UINT32 CLAMP_DISK_CH		  =  4;
	static const UINT32 PICK_UP_DISK_CH	      =  5;
	static const UINT32 STOW_LOADER_CH        =  6;
	static const UINT32 TIPPER_SW_CH		  =  7;

	// Joystick Buttons
	static const UINT32 TRAN_LOW_SPEED_BT     =  2;  // Transmission gear shift
	static const UINT32 TRAN_HIGH_SPEED_BT    =  3;
	static const UINT32 TRAN_GRANNY_BT		  =  8;


	//----------------------------------------------------------------------
	// ROBOT MODULES AND CHANNELS - INPUTS AND OUTPUTS
	//----------------------------------------------------------------------
	// cRIO Module Assignments - Robot Inputs & Outputs
	static const UINT8  ANALOG_BREAKOUT_MODULE   = 1;
	static const UINT8  DIGITAL_SIDECAR_MODULE   = 1;
	static const UINT8  SOLENOID_BREAKOUT_MODULE = 1;

	// cRIO Digital Sidecar Channels - Robot PWM Controls (Outputs)
	// Victor motors with speed speed control
	// PWM = Pulsed width modulation

	// cRIO Digital Sidecar Channels - Robot Relay Controls (Outputs)
	static const UINT32 COMPRESSOR_CH         =  1;  // Compressor on/off
	static const UINT32 CAMERA_LIGHTS_CH      =  2;


	// cRIO Digital Sidecar Channels - GPIO & Spare Power Outputs
	// Robot inputs
	// - Air compressor pressure switch (indicates when pressure is
	//   too high)
	static const UINT32 PRESSURE_SW_CH         =  1;  // Compressor pressure switch
	static const UINT32 AUTO_MODE_CH           =  2;
	static const UINT32 AM_SW1_CH              =  3;
	static const UINT32 AM_SW2_CH              =  4;
	static const UINT32 AM_SW3_CH              =  5;

	//----------------------------------------------------------------------
	// CONSTANTS USED TO DETERMINE STATUS OF DRIVER STATION AND ROBOT INPUTS
	// AND TO INSTRUCT ROBOT
	// Private static constants used in multiple methods
	//----------------------------------------------------------------------
	// CONSTANTS USED IN DECLARING OBJECTS
	//----------------------------------------------------------------------
	// Encoder direction
	static const bool   ENCODER_NORMAL_DIR  = false;
	static const bool   ENCODER_REVERSE_DIR = true;
	// Transmission constants
	static const int    HIGH_GEAR            =  2;
	static const int    LOW_GEAR             =  1;
	static const int    GRANNY_GEAR          =  0;
	static const float  NORMAL_SPEED_ADJ     =  1.0;
	static const float  GRANNY_SPEED_ADJ     =  0.3;
	static const float  SLOW_REVERSE_SPEED   = -0.30;
	static const float  SLOW_FORWARD_SPEED   =  0.19;
	static const float  STOP_MOTOR           =  0.0;

	// Initialize wheel encoder variables
	// 19.5" per wheel rotation
	// 250 pulses per rotation of encoder
	// Gear ratio between encoder shaft and wheel shaft 24/36
	// = 19.50 / 250 * ( 24/36)
	static const double ELEV_DIST_PER_PULSE     = 0.052;

	static const double WHEEL_DIST_PER_PULSE    = 0.052;

	// Ball Loader Constants
	static const float  ANGLE_UP_SPEED       =  0.2;
	static const float  ANGLE_DOWN_SPEED     = -0.2;

	//Object Formula Variables
	static const float  AIMER_CONST			 = 9.61;
	static const float	AIMER_RATIO			 = 17.6;

	static const int targetPickUp =  1000;
	static const int targetLoad	  =   385;
	static const int targetStow	  =     0;

	//----------------------------------------------------------------------
	// AUTONOMOUS MODE ROBOT CONTROL CONSTANTS (OUTPUTS)
	//----------------------------------------------------------------------

	//----------------------------------------------------------------------
	// AUTONOMOUS MODE ROBOT STATE & TIMING TRACKING
	// Used to determine what robot is or should be doing in autonomous mode
	//----------------------------------------------------------------------
	// Autonomous Mode States
	static const float     AUTO_DRIVE_SPEED	  =  .2;
	static const float	   AUTO_REVERSE_SPEED = -.2;
	//----------------------------------------------------------------------
	// POINTERS FOR REQUIRED OBJECTS
	//----------------------------------------------------------------------
	// DRIVER STATION INPUT & OUTPUT POINTERS
	//----------------------------------------------------------------------
	// Includes driver station laptop, joysticks, switches and other digital
	// and analog devices connected through the eStop Robotics CCI.
	//----------------------------------------------------------------------
	DriverStation	 *pDriverStation;
	DriverStationLCD *pDSLCD;
	SmartDashboard   *pDashboard;

	Joystick		 *pRightStick;			// joystick 1 (right tank stick)
	Joystick		 *pLeftStick;	        // joystick 2 (left tank stick)
	JoystickButton   *pHighSpeedRightBt;
	JoystickButton   *pHighSpeedLeftBt;
	JoystickButton   *pLowSpeedRightBt;
	JoystickButton   *pLowSpeedLeftBt;
	JoystickButton   *pGrannyRightBt;
	JoystickButton   *pGrannyLeftBt;
	Joystick         *pCCI;
	JoystickButton	 *pCameraLightSwitch;
	JoystickButton	 *pClampSwitch;
	JoystickButton	 *pPickUpSwitch;
	JoystickButton	 *pStowSwitch;
	JoystickButton	 *pAutoAimSwitch;
	JoystickButton	 *pShootSwitch;
	JoystickButton	 *pTipperSwitch;

	//----------------------------------------------------------------------
	// ROBOT INPUT & OUTPUT POINTERS
	//----------------------------------------------------------------------
	//----------------------------------------------------------------------
	// Robot Digital Inputs - GPIO Inputs including Encoders
	//----------------------------------------------------------------------
	DigitalInput    *pAMDriveSwitch;
	DigitalInput    *pAMPickUpSwitch;
	DigitalInput    *pAMLeftSwitch;
	DigitalInput    *pAMRightSwitch;

	//----------------------------------------------------------------------
	// Robot Digital Outputs - Relays (Compressor & Spikes)
	//----------------------------------------------------------------------
	Compressor      *pCompressor;           // Air compressor
	CameraLights	*pCameraLights;			// Camera LED lights
	//----------------------------------------------------------------------
	// Robot Objects
	//----------------------------------------------------------------------
	DiskLoader		*pDiskLoader;
	DiskStorage		*pDiskStorage;
	RobotAimer	    *pRobotAimer;
	DiskShooter		*pDiskShooter;
	Elevator		*pElevator;
	Tipper			*pTipper;

	//----------------------------------------------------------------------
	// VARIABLES USED IN CLASS
	//----------------------------------------------------------------------
	// DRIVER STATION INPUTS - Analog Inputs from eStop Robotics CCI
	//----------------------------------------------------------------------
	// Ball Launcher target rotations per second
	// - Manual input based on potentionmeter setting
	//----------------------------------------------------------------------

	//----------------------------------------------------------------------
	// DRIVER STATION INPUTS - Digital Inputs from eStop Robotics CCI
	//----------------------------------------------------------------------
	// Camera Switches
	bool   lightsOn;
	// Ball Launcher Switches
	bool   autoAimOn;
	bool   shootOneOn;
	// DiskClampSwitches
	bool   clampDiskOn;
	bool   pickUpOn;
	bool   stowOn;
	// Shooter Angle Switch
	bool   angleUpOn;
	bool   angleDownOn;
	// Image Processing
	bool   targetFound;
	// Tipper
	bool   tipOn;
	//----------------------------------------------------------------------
	// CLASS VARIABLES USED TO TRACK ROBOT STATUS
	//----------------------------------------------------------------------
	// General status tracking
	//----------------------------------------------------------------------
	// Class variables to track look (packet) counts
	UINT32 loopCount;
	float  loopsPerMinute;

	// Class variables to track time
	INT32  startSec;
	INT32  elapsedSec;
	INT32  oldSec;
	//----------------------------------------------------------------------
	// Camera Image Processing
	//----------------------------------------------------------------------
	bool aimedX;
	bool aimedY;
	//----------------------------------------------------------------------
	// Transmission Speed / Shifting
	//----------------------------------------------------------------------
	int    tranSpeed;            // Transmission speed
	float  leftDriveSpeed;
	float  rightDriveSpeed;

	//----------------------------------------------------------------------
	// Wheel encoders
	//----------------------------------------------------------------------
	double rightDistance;
	double leftDistance;

	//----------------------------------------------------------------------
	// Disk Loading Drive
	//----------------------------------------------------------------------
	int		diskAction;

	//----------------------------------------------------------------------
	// Disk Shooting
	//----------------------------------------------------------------------
	bool   autoAimTargetFound;
	bool   startShooting;
	bool   lastInFiringCycle;
	bool   inShootingCycle;
	bool   inFiringCycle;
	float  shooterAngleTarget;

	//----------------------------------------------------------------------
	// Autonomous Mode Switches & variables
	//----------------------------------------------------------------------
	bool AMDriving;
	bool AMPickingUp;
	int  AMPosition;

	bool AMDrivingFinished;
	bool AMFinishedShooting;
	bool AMShooting;
	int  AMShootCount;
	int  AMShootCountMax;

	enum position{left, middle, right};

};
//------------------------------------------------------------------------------
// INITIALIZE STATIC CONSTANTS
//------------------------------------------------------------------------------

START_ROBOT_CLASS(BuiltinDefaultCode);

//------------------------------------------------------------------------------
// METHOD DEFINITONS
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::BuiltinDefaultCode
// Type:	Public default constructor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Defines pointers to required robot objects and initializes packet count and
// loop count variables.
//------------------------------------------------------------------------------
BuiltinDefaultCode::BuiltinDefaultCode()
{
	//----------------------------------------------------------------------
	// DEFINE POINTERS TO REQUIRED ROBOT OBJECTS
	//----------------------------------------------------------------------
	// Acquire the Driver Station object
	pDriverStation = DriverStation::GetInstance();
	pDSLCD		   = DriverStationLCD::GetInstance();

	// Define joysticks & CCI
	pRightStick		    = new Joystick(RIGHT_JS_PORT);
	pLeftStick			= new Joystick(LEFT_JS_PORT);
	pCCI                = new Joystick(CCI_PORT);       // CCI uses joystick object

	pHighSpeedRightBt   = new JoystickButton(pRightStick,TRAN_HIGH_SPEED_BT);
	pHighSpeedLeftBt    = new JoystickButton(pLeftStick,TRAN_HIGH_SPEED_BT);
	pLowSpeedRightBt    = new JoystickButton(pRightStick,TRAN_LOW_SPEED_BT);
	pLowSpeedLeftBt     = new JoystickButton(pLeftStick,TRAN_LOW_SPEED_BT);
	pGrannyRightBt      = new JoystickButton(pRightStick,TRAN_GRANNY_BT);
	pGrannyLeftBt       = new JoystickButton(pLeftStick,TRAN_GRANNY_BT);
	pCameraLightSwitch  = new JoystickButton(pCCI,LED_LIGHTS_ON);
	pClampSwitch        = new JoystickButton(pCCI,CLAMP_DISK_CH);
	pPickUpSwitch       = new JoystickButton(pCCI,PICK_UP_DISK_CH);
	pStowSwitch			= new JoystickButton(pCCI,STOW_LOADER_CH);
	pAutoAimSwitch		= new JoystickButton(pCCI,AUTO_TARGET_CH);
	pShootSwitch		= new JoystickButton(pCCI,SHOOT_DISK_CH);
	pTipperSwitch		= new JoystickButton(pCCI,TIPPER_SW_CH);


	// Victor Motor Controllers (PMW Connections)
	// Motors requiring speed controls (drive, ball launcher, rollers)

	// Spike Relays (Relay Connections)
	// Motors without speed control, compressor, lights
	pCompressor         = new Compressor(PRESSURE_SW_CH,COMPRESSOR_CH);
	pCameraLights		= new CameraLights(DIGITAL_SIDECAR_MODULE,CAMERA_LIGHTS_CH);
	// GPIO & Spare Power Outputs	// Inputs from robot (compressor pressure switch, limit switches, autonomous
	// mode switches)
	pAMDriveSwitch           = new DigitalInput(DIGITAL_SIDECAR_MODULE,AUTO_MODE_CH);
	pAMPickUpSwitch          = new DigitalInput(DIGITAL_SIDECAR_MODULE,AM_SW1_CH);
	pAMLeftSwitch      		 = new DigitalInput(DIGITAL_SIDECAR_MODULE,AM_SW2_CH);
	pAMRightSwitch  	     = new DigitalInput(DIGITAL_SIDECAR_MODULE,AM_SW3_CH);


	pDriveMotors		= new TankDrive();
	// Disk Manipulation Objects
	pDiskLoader			= new DiskLoader();
	pDiskStorage		= new DiskStorage();
	pDiskShooter		= new DiskShooter();

	// Camera
	pRobotAimer			= new RobotAimer(pDriveMotors);

	// Elevator
	pElevator 			= new Elevator();

	// Tipper
	pTipper				= new Tipper();

	/* PID Controllers
//	pBLControl     = new SendablePIDController(iProportional
	pBLControl     = new PIDController(iProportional
										           ,iIntegral
			                                       ,iDerivative
			                                       ,pBLEncoder
			                                       ,pBLMotor
			                                       ,iPIDInterval);
	 */
	//----------------------------------------------------------------------
	// INITIALIZE VARIABLES
	//----------------------------------------------------------------------
	// Initialize loop and time counters
	loopCount  = 0;
	startSec   = 0;
	elapsedSec = 0;


	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::~BuiltinDefaultCode
// Type:	Public default destructor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
BuiltinDefaultCode::~BuiltinDefaultCode()
{
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::RobotInit()
// Type:	Performs robot initiation functions.  Overrides RobotInit() virtual
//          method contained in WPILib.
//
//			These actions are performed once and only once when the robot is
//	        powered on.
//------------------------------------------------------------------------------
// Functions:
// - Turns air compressor on
// - Shifts robot transmission into low gear
//------------------------------------------------------------------------------
void BuiltinDefaultCode::RobotInit()
{
	GetWatchdog().SetExpiration(0.5);
	SmartDashboard::init();

	autoAimTargetFound  = false;
	startShooting 		= false;
	lastInFiringCycle 	= false;
	inShootingCycle 	= false;
	inFiringCycle 		= false;
	shooterAngleTarget 	= 20;

	pElevator->SetDistancePerPulse(ELEV_DIST_PER_PULSE);
	pDriveMotors->SetDistancePerPulse(WHEEL_DIST_PER_PULSE);
	pRobotAimer->SetInputToOutputFormula(AIMER_CONST,AIMER_RATIO);
	pDiskLoader->SetLoaderTargets(targetPickUp,targetLoad,targetStow);

	// Turn compressor for pneumatics on
	pCompressor->Start();

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::DisabledInit()
// Type:	Executes when the robot is placed in Disabled mode.  Overrides
//			DisabledInit() virtual method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Resets loop counter for disabled mode
// - Starts the clock that counts how long robot has been in disabled mode
//------------------------------------------------------------------------------
void BuiltinDefaultCode::DisabledInit()
{
	// Reset loop counter
	loopCount  = 0;

	// Capture time disabled mode was entered
	elapsedSec = 0;
	startSec   = (INT32)GetClock();

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::AutonomousInit()
// Type:	Executes when the robot is placed in Autonomous mode.  Overrides
//			AutonomousInit() virtual method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Resets the loop counter for autonomous mode
// - Resets the AutoState
// - Resets the encoders on the wheels that measure distance traveled.
// - Shifts the transmission into low gear
// - Optionally prints the status of the autonomous mode switches for debugging
//   purposes
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AutonomousInit()
{
	// Reset loop counter
	loopCount  = 0;

	// Capture time autonomous mode was entered
	elapsedSec = 0;
	startSec   = (INT32)GetClock();


	GetRobotSensorInput();
	GetAutoModeSwitches();

	pDriveMotors->ResetWheelEncoders();



	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::TeleopInit()
// Type:	Executes when the robot is placed in Teleoperated mode.  Overrides
//			TeleopInit() virtual method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Resets the loop and packet counters for teleoperated mode
// - Resets the distance tracking variables
// - Resets the wheel encoder counters
// - Obtain the current position of the elevator from the robot
//------------------------------------------------------------------------------
void BuiltinDefaultCode::TeleopInit()
{
	// General loop count & elapsed time initialization
	loopCount      = 0;
	loopsPerMinute = 0;

	elapsedSec = 0;
	startSec   = (INT32)GetClock();
	oldSec     = startSec;

	pDSLCD->Printf(DriverStationLCD::kMain_Line6, 1, "TeleopInit2");
	pDSLCD->UpdateLCD();

	GetDriverStationInput();
	GetRobotSensorInput();



	pDriveMotors->ResetWheelEncoders();


	//	ResetGyro();


	//	ballsOnRobot = 0;

}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::DisabledPeriodic()
// Type:	Executes when the robot is in disabled mode.  Overrides the
//			DisabledPeriodic() virtual method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Define a variable that captures the time this method is invoked.
// - Feed watchdog to prevent robot shut-down
// - Increment the disabled loop counter
// - Increments the time the robot is in disabled mode
// - Optionally can print to the console the time robot is in disabled mode
//------------------------------------------------------------------------------
void BuiltinDefaultCode::DisabledPeriodic()
{
	// Feed the user watchdog at every period when disabled
	GetWatchdog().Feed();

	// Increment & display loop counter
	loopCount++;

	// Calculate & display elapsed seconds
	elapsedSec = (INT32)GetClock() - startSec;

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::AutonomousPeriodic()
// Type:	Executes when the robot is in autonomous mode.  Overrides the
//			AutonomousPeriodic() virtual method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Increments the count of loops while in autonomous mode.
// - Feeds to watchdog to prevent robot shut-down
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AutonomousPeriodic()
{
	// Feed the user watchdog at every period when disabled
	GetWatchdog().Kill();

	// Increment & display loop counter
	loopCount++;

	// Calculate & display elapsed seconds
	elapsedSec = (INT32)GetClock() - startSec;

	tranSpeed = Transmission::kLowSpeed;

	GetRobotSensorInput();

	RunAutonomousMode();

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::TeleopPeriodic()
// Type:	Executes when the robot is in teleoperated mode each time a new
//          packet of information has been received by the Driver Station.  Any
//          code which needs new information from the Driver Station should be
//          placed here.  This method overrides the TeleopPeriodic() virtual
//          method contained in WPILib.
//------------------------------------------------------------------------------
// Functions:
// - Increments the count of loops processed and packets received while
//   in teleoperated mode.
// - Feeds to watchdog to prevent robot shut-down
// - Obtains input from the driver station (joystick inputs, switches, arm
//   rotator potentiometer)
// - If a transmission shift was requested by the driver station, shift
//   transmission
// - Sets the drive motor values based on joystick movement
// - Sets the ball launcher motor speed values based on on/off switch and
//   potentiometer position
//------------------------------------------------------------------------------
void BuiltinDefaultCode::TeleopPeriodic()
{
	// Feed the user watchdog at every period when disabled
	GetWatchdog().Kill();

	// Increment & display loop counter
	loopCount++;
	pDSLCD->Printf(DriverStationLCD::kMain_Line6, 1, "TeleopPeriodic");
	pDSLCD->UpdateLCD();


	// Calculate & display elapsed seconds
	elapsedSec = (INT32)GetClock() - startSec;

	// Get inputs from the driver station
	GetDriverStationInput();

	// Get robot sensor input
	GetRobotSensorInput();

	SmartDashboard::PutNumber("tranSpeed", tranSpeed);

	pDriveMotors->SetDriveMotors(tranSpeed, leftDriveSpeed, rightDriveSpeed);
	pRobotAimer->AdjustShooterAngle(shooterAngleTarget);

	// Set drive motor speed and direction based on joysticks (left & right)

	if (clampDiskOn)
		pDiskLoader->ClampDisk();
	else
		pDiskLoader->UnClampDisk();

	if(tipOn)
		pTipper->TipRobot();
	else
		pTipper->UnTipRobot();

	if(pickUpOn)
		pDiskLoader->MoveLoader(DiskLoader::PickUpDisk);
	else if (stowOn)
		pDiskLoader->MoveLoader(DiskLoader::StowLoader);
	else
		pDiskLoader->MoveLoader(DiskLoader::FeedDisk);

	// Move the elevator if necessary
	pElevator->MoveElevator(pCCI->GetY());

	// Turn camera LED lights on or off
	if ( lightsOn )
		pCameraLights->TurnOn();
	else
		pCameraLights->TurnOff();


	if(startShooting || inFiringCycle)
	{
		inFiringCycle = pDiskShooter->ShootOneDisk();
		startShooting = false;
		if (!inFiringCycle)
			inShootingCycle = false;
	}

	if(!inFiringCycle)
	{
		diskAction = pDiskStorage->DetermineDiskAction();

		if(diskAction == DiskStorage::DisksToLoad)
			pDiskStorage->LoadDisks();
		else if(diskAction == DiskStorage::DisksToReverse)
			pDiskStorage->ReverseDisks();
		else if(diskAction == DiskStorage::DisksToReady)
			pDiskStorage->ReadyDisksForShooting();
	}

	if (shootOneOn || inShootingCycle)
	{
		inShootingCycle = true;
		if(!inFiringCycle)
		{
			if(autoAimOn)
			{
				autoAimTargetFound = pRobotAimer->ProcessImage();
				if(autoAimTargetFound)
				{
					aimedX = pRobotAimer->AimRobotX();
					aimedY = pRobotAimer->AimRobotY();
					if (aimedX && aimedY)
						startShooting = pDiskStorage->ReadyDisksForShooting();
				}
			}
			else
			{
				startShooting = pDiskStorage->ReadyDisksForShooting();
			}
		}
	}


#ifdef CONSOLE
	// Display SmartDashboard Values
	SmartDashboard::PutBoolean("Auto aiming",autoAimOn);
	SmartDashboard::PutBoolean("Auto Aim target found",autoAimTargetFound);
	SmartDashboard::PutNumber("Disk Action is: ", diskAction);
#endif
	return;
}

//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetDriverStationInput()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Obtains the input from the DriverStation required for teleoperated mode.
// Includes obtaining input for the following switches:
// - Ball Launcher Motor On (SPSD fitch)
// May also need to include reading additional switches depending on where
// functions are included on the robot or driver station.
// - Transmission High/Low speed (SPST switch)
// Include also:
// - Reading joystick functions (left and right)
// - Reading arm rotation potentiometer (analog input)
// Does not include:
// - Tank drive input from the joysticks.  This is coded directly in the
//   SetDriveMotors() method.
//------------------------------------------------------------------------------
void BuiltinDefaultCode::GetDriverStationInput()
{
	if ( pHighSpeedLeftBt->Get() &&
			pHighSpeedRightBt->Get() )
	{
		tranSpeed = Transmission::kHighSpeed;
	}
	else
	{
		if ( pLowSpeedLeftBt->Get() &&
				pLowSpeedRightBt->Get() )
		{
			tranSpeed  = Transmission::kLowSpeed;
		}
		else if ( pGrannyLeftBt->Get() ||
				pGrannyRightBt->Get() )
		{
			tranSpeed  = GRANNY_GEAR;
		}
	}

	// Joystick Drive Values
	leftDriveSpeed  = pLeftStick->GetY();
	rightDriveSpeed = pRightStick->GetY();
	shooterAngleTarget = pCCI->GetX();

	// Obtain the position of switches on the driver station
	// Camera Switches
	lightsOn  				 = pCameraLightSwitch->Get();

	// Ball Launching Switches
	autoAimOn				 = pAutoAimSwitch->Get();
	shootOneOn				 = pShootSwitch->Get();

	// Bridge Tipper Switches
	clampDiskOn   			 = pClampSwitch->Get();
	pickUpOn			     = pPickUpSwitch->Get();
	stowOn					 = pStowSwitch->Get();
	tipOn					 = pTipperSwitch->Get();


	ShowDSValues();

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::ShowDSValues()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void BuiltinDefaultCode::ShowDSValues()
{
	// Show the values for driver station inputs

	// Show Joystick Buttons for Shifting
	SmartDashboard::PutBoolean("Right JS Button 3",pHighSpeedRightBt->Get());
	SmartDashboard::PutBoolean("Left JS Button 3",pHighSpeedLeftBt->Get());
	SmartDashboard::PutBoolean("Left JS Button 2",pLowSpeedLeftBt->Get());
	SmartDashboard::PutBoolean("Right JS Button 2",pLowSpeedRightBt->Get());
	SmartDashboard::PutBoolean("Right JS Button 8",pGrannyRightBt->Get());
	SmartDashboard::PutBoolean("Left JS Button 8",pGrannyLeftBt->Get());
	SmartDashboard::PutNumber("DS Pot Value", shooterAngleTarget);

	// Camera Light Switch Value

	return;
}

//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::ShowRobotValues()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void BuiltinDefaultCode::ShowRobotValues()
{
	// Show the values from the robot components

	// Show Motor Speed
	SmartDashboard::PutNumber("Left Motor Speed:"
			,pDriveMotors->GetMotorSpeed(TankDrive::kLeftWheels));
	SmartDashboard::PutNumber("Right Motor Speed:"
			,pDriveMotors->GetMotorSpeed(TankDrive::kRightWheels));

	// Show Encoder Distances
	SmartDashboard::PutNumber("Left Distance:"
			,leftDistance);
	SmartDashboard::PutNumber("Right Distance:"
			,rightDistance);
	SmartDashboard::PutNumber("Transmission Speed", tranSpeed);

	// Show Autonomous Switches
	SmartDashboard::PutBoolean("AM Driving", AMDriving);
	SmartDashboard::PutBoolean("AM Picking Up", AMPickingUp);
	SmartDashboard::PutNumber("AM Position", AMPosition);

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetRobotSensorInput()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Looks at data in the packet and obtains input coming from the robot to be
// used in both Autonomous and Teleoperated Modes.
// - Distance traveled by right wheels
// - Distance traveled by left wheels
//------------------------------------------------------------------------------
void BuiltinDefaultCode::GetRobotSensorInput()
{
	// Get distanced traveled by encoder
	rightDistance   = pDriveMotors->GetDistance(TankDrive::kRightWheels);
	leftDistance    = pDriveMotors->GetDistance(TankDrive::kLeftWheels);



	ShowRobotValues();
	return;
}

//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetAutoModeSwitches()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void BuiltinDefaultCode::GetAutoModeSwitches()
{
	/* Get Autonomous Switches
		if(pAMDriveSwitch->Get())
		{
			AMDriving = true;
		}
		else
		{
			AMDriving = false;
		}

		if(pAMPickUpSwitch->Get())
		{
			AMPickingUp = true;
		}
		else
		{
			AMPickingUp = false;
		}

		if(pAMLeftSwitch->Get())
		{
			AMPosition = left;
		}
		else if(pAMRightSwitch->Get())
		{
			AMPosition = right;
		}
		else
		{
			AMPosition = middle;
		}

		ShowRobotValues();
	// Initialize to autonomous mode off
	autonomousModeOn = false;
	amAutoSpeed      = false;
	amAutoAim        = false;
	amDriveTiming    = AM_NO_DRIVING;
	amDriveMethod    = AM_NO_DRIVING;

	// Mode 00: Autonomous mode off, manual aim, manual shoot, no driving
	if ( !pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		 !pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = false;
		amAutoSpeed      = false;
		amAutoAim        = false;
		amDriveTiming    = AM_NO_DRIVING;
		amDriveMethod    = AM_NO_DRIVING;
	}

	// Mode 01: Autonomous mode is off
	if ( !pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
    	autonomousModeOn = false;
    	amAutoSpeed      = false;
    	amAutoAim        = false;
    	amDriveTiming    = AM_NO_DRIVING;
    	amDriveMethod    = AM_NO_DRIVING;
	}

	// Mode 02: Autonomous mode on, manual aim, manual shoot, drive after
    //          using Kinect
	if ( !pAMSwitch1->Get()      &&
		  pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		 !pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = false;
		amAutoAim        = false;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_KINECT_DRIVE;
	}

   	// Mode 03: Autonomous mode on, manual aim, manual shoot, no driving
    if ( !pAMSwitch1->Get()      &&
   		  pAMSwitch2->Get()      &&
   		  pAMSwitch3Upper->Get() &&
   		  pAMSwitch3Lower->Get()     )
   	{
   		autonomousModeOn = true;
   		amAutoSpeed      = false;
   		amAutoAim        = false;
   		amDriveTiming    = AM_NO_DRIVING;
   		amDriveMethod    = AM_NO_DRIVING;
   	}

    // Mode 04: Autonomous mode on, manual aim, manual shoot, drive after
    //          automatically
	if ( !pAMSwitch1->Get()      &&
		  pAMSwitch2->Get()      &&
		 !pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = false;
		amAutoAim        = false;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_AUTO_DRIVE;
	}

	// Mode 05: Autonomous mode on, manual aim, automatic shoot, drive after
	//          using Kinect
	if (  pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		 !pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = false;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_KINECT_DRIVE;
	}

	// Mode 06: Autonomous mode on, manual aim, automatic shoot, no driving
	if (  pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = false;
		amDriveTiming    = AM_NO_DRIVING;
		amDriveMethod    = AM_NO_DRIVING;
	}

	// Mode 07: Autonomous mode on, manual aim, automatic shoot, drive after
	//          automatically
	if (  pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		 !pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = false;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_AUTO_DRIVE;
	}

	// Mode 08: Autonomous mode on, auto aim, automatic shoot, drive after
	//          using kinect
	if (  pAMSwitch1->Get()      &&
		  pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		 !pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = true;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_KINECT_DRIVE;
	}

	// Mode 09: Autonomous mode on, auto aim, automatic shoot, no driving
	if (  pAMSwitch1->Get()      &&
		  pAMSwitch2->Get()      &&
		  pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = true;
		amDriveTiming    = AM_NO_DRIVING;
		amDriveMethod    = AM_NO_DRIVING;
	}

	// Mode 10: Autonomous mode on, auto aim, automatic shoot, drive after
	//          automatically
	if (  pAMSwitch1->Get()      &&
		  pAMSwitch2->Get()      &&
		 !pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = true;
		amDriveTiming    = AM_DRIVE_AFTER;
		amDriveMethod    = AM_AUTO_DRIVE;
	}

	// Mode 11: Autonomous mode on, auto aim, automatic shoot, drive before
	//          automatically
	if ( !pAMSwitch1->Get()      &&
		 !pAMSwitch2->Get()      &&
		 !pAMSwitch3Upper->Get() &&
		  pAMSwitch3Lower->Get()     )
	{
		autonomousModeOn = true;
		amAutoSpeed      = true;
		amAutoAim        = true;
		amDriveTiming    = AM_DRIVE_BEFORE;
		amDriveMethod    = AM_AUTO_DRIVE;
	}

	autoAimOn   = amAutoAim;

	 */
	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetTime()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Shifts gear on transmission to match input from driver station.  Gear shifts
// are via a pneumatic cylinder controlled by a solenoid.
//------------------------------------------------------------------------------
INT32 BuiltinDefaultCode::GetTime()
{
	INT32  currentTime;

	currentTime = (INT32)GetClock();

	return currentTime;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::RunAutonomousMode()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void BuiltinDefaultCode::RunAutonomousMode()
{

	if((AMDriving && !AMDrivingFinished) && AMPosition != middle)
	{
		AMDriveRobot();
	}
	else if (!AMFinishedShooting)
	{
		AMShootOriginalDisks();
	}
	else if (AMPickingUp && !AMShooting)
	{
		AMPickUpDisk();
	}
	else if(AMPickingUp)
	{
		AMShootExtraDisks();
	}
	/*
	if ( amDriveTiming == AM_DRIVE_AFTER )
	{
		if ( shootingBalls )
		{
			AimAndShootBalls();
			if ( ballLaunched )
			{
				DetermineBallLocation();
				LoadNextBall();
			}
		}
		else
		{
			StopPIDController();
			StopLauncherMotor();
			StopLaunchRoller();
			AMDriveRobot();
		}
	}
	else
	{
		if ( amDriveTiming == AM_DRIVE_BEFORE )
		{
			AMDriveRobot();
			if ( shootingBalls )
			{
				AimAndShootBalls();
				if ( ballLaunched )
				{
					DetermineBallLocation();
					LoadNextBall();
				}
			}
			else
			{
				StopPIDController();
				StopLauncherMotor();
				StopLaunchRoller();
			}
		}
		else
		{
			if ( shootingBalls )
			{
				AimAndShootBalls();
				if ( ballLaunched )
				{
					DetermineBallLocation();
					LoadNextBall();
				}
			}
			else
			{
				StopPIDController();
				StopLauncherMotor();
				StopLaunchRoller();
			}
		}
	}
	 */
	return;
}

//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetTime()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Shifts gear on transmission to match input from driver station.  Gear shifts
// are via a pneumatic cylinder controlled by a solenoid.
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AMDriveRobot()
{
	AMDrivingFinished = false;

	if (pDriveMotors->GetDistance(TankDrive::kLeftWheels) < 117.5)
	{
		pDriveMotors->SetDriveMotors(tranSpeed,AUTO_DRIVE_SPEED,AUTO_DRIVE_SPEED);
	}
	else if(AMPosition == left && pDriveMotors->GetDistance(TankDrive::kRightWheels) < 160)
	{
		pDriveMotors->SetDriveMotors(tranSpeed,AUTO_REVERSE_SPEED,AUTO_DRIVE_SPEED);
	}
	else if (AMPosition == right && pDriveMotors->GetDistance(TankDrive::kLeftWheels) < 160)
	{
		pDriveMotors->SetDriveMotors(tranSpeed,AUTO_DRIVE_SPEED,AUTO_REVERSE_SPEED);
	}
	else
	{
		pDriveMotors->SetDriveMotors(tranSpeed,STOP_MOTOR,STOP_MOTOR);
		AMDrivingFinished = true;
	}

	return;
}

//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::GetTime()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Shifts gear on transmission to match input from driver station.  Gear shifts
// are via a pneumatic cylinder controlled by a solenoid.
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AMShootOriginalDisks()
{
	if(AMPosition == left ||AMPosition == right)
		AMShootCountMax = 3;
	else
		AMShootCountMax = 2;

	AMFinishedShooting = false;

	if (AMShootCount < AMShootCountMax)
	{
		if(!inFiringCycle)
		{
			diskAction = pDiskStorage->DetermineDiskAction();

			if (diskAction = DiskStorage::DisksToReady)
				startShooting = pDiskStorage->ReadyDisksForShooting();
			else
			{
				pRobotAimer->ProcessImage();
				aimedX = pRobotAimer->AimRobotX();
				aimedY = pRobotAimer->AimRobotY();

				if (aimedX && aimedY)
					startShooting = pDiskStorage->ReadyDisksForShooting();
			}
		}
		if (startShooting || inFiringCycle)
		{
			inFiringCycle = pDiskShooter->ShootOneDisk();
			startShooting = false;
		}

		if (lastInFiringCycle && !inFiringCycle)
		{
			AMShootCount++;
		}
	}
	else
		AMFinishedShooting = true;

	lastInFiringCycle = inFiringCycle;

	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::AMPickUpDisks()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Shifts gear on transmission to match input from driver station.  Gear shifts
// are via a pneumatic cylinder controlled by a solenoid.
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AMShootExtraDisks()
{
	return;
}
//------------------------------------------------------------------------------
// METHOD:  BuiltinDefaultCode::AMPickUpDisks()
// Type:	Public accessor for BuiltinDefaultCode class
//------------------------------------------------------------------------------
// Shifts gear on transmission to match input from driver station.  Gear shifts
// are via a pneumatic cylinder controlled by a solenoid.
//------------------------------------------------------------------------------
void BuiltinDefaultCode::AMPickUpDisk()
{
	return;
}
